/*
 *  ProtoBufTest.cpp
 *  PhysicsTest
 *
 *  Created by Micah Pearlman on 12/16/08.
 *  Copyright 2008 Monk Games. All rights reserved.
 *
 */

#include "ProtoBufTest.h"

#include "GlutStuff.h"
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"
#include "btTransform.h"
#include <stdio.h> //printf debugging
#include <iostream>
#include <fstream>

using namespace std;

///create 125 (5x5x5) dynamic object
#define ARRAY_SIZE_X 5
#define ARRAY_SIZE_Y 5
#define ARRAY_SIZE_Z 5

//maximum number of objects (and allow user to shoot additional boxes)
#define MAX_PROXIES (ARRAY_SIZE_X*ARRAY_SIZE_Y*ARRAY_SIZE_Z + 1024)

///scaling of the objects (0.1 = 20 centimeter boxes )
#define SCALING 1.0
#define START_POS_X -5
#define START_POS_Y -5
#define START_POS_Z -3

void ProtoBufTest::clientMoveAndDisplay()
{
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 
	
	//simple dynamics world doesn't handle fixed-time-stepping
	float ms = getDeltaTimeMicroseconds();
	
	///step the simulation
	if (m_dynamicsWorld)
	{
		m_dynamicsWorld->stepSimulation(ms / 1000000.f);
		//optional but useful: debug drawing
		m_dynamicsWorld->debugDrawWorld();
	}
	
	renderme(); 
	
	glFlush();
	
	glutSwapBuffers();
	
}



void ProtoBufTest::displayCallback(void) {
	
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 
	
	renderme();
	
	//optional but useful: debug drawing to detect problems
	if (m_dynamicsWorld)
		m_dynamicsWorld->debugDrawWorld();
	
	glFlush();
	glutSwapBuffers();
}





void	ProtoBufTest::initPhysics()
{
	setTexturing(true);
	setShadows(true);
	
	
	
	///collision configuration contains default setup for memory, collision setup
	m_collisionConfiguration = new btDefaultCollisionConfiguration();
	
	///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
	m_dispatcher = new	btCollisionDispatcher(m_collisionConfiguration);
	
	m_broadphase = new btDbvtBroadphase();
	
	///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
	btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
	m_solver = sol;
	
	m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
	
	


	// read in each protocol buffer
	{
		fstream input( "../jenga.dae_physics_material_library.pb", ios::in | ios::binary );
		if( !input )
			cout << "error opening file" << endl;
		else if ( physics_material_library_.ParseFromIstream( &input ) ) {
			cout << "succesfully read physics material library" << endl;
		}
		else {
			cout << "error reading physics material library" << endl;
		}
	}

	{
		fstream input( "../jenga.dae_physics_model_library.pb", ios::in | ios::binary );
		if( !input )
			cout << "error opening file" << endl;
		else if ( physics_model_library_.ParseFromIstream( &input ) ) {
			cout << "succesfully read physics model library" << endl;
		}
		else {
			cout << "error reading physics model library" << endl;
		}
	}
	
	{
		fstream input( "../jenga.dae_physics_scene_library.pb", ios::in | ios::binary );
		if( !input )
			cout << "error opening file" << endl;
		else if ( physics_scene_library_.ParseFromIstream( &input ) ) {
			cout << "succesfully read physics scene library" << endl;
		}
		else {
			cout << "error reading physics scene library" << endl;
		}
	}
	
	// now parse through the protocol buffer physics scene and create bullet physics scene
	const ::collada_to_protobuf::physics_scene &physicsScene = physics_scene_library_.physics_scenes_( 0 ); // just grab the first physics scene

	btVector3 gravity( physicsScene.gravity_().x(), physicsScene.gravity_().y(), physicsScene.gravity_().z() );
	m_dynamicsWorld->setGravity( -1.0f * gravity );
	// setup camera based on gravity
	setCameraForwardAxis(0);
	gravity = gravity.normalize();
	setCameraUp(-1.0f * gravity );
	setCameraDistance(btScalar(SCALING*110.));
	for( int i = 0; i < physicsScene.instance_physics_models__size(); i++ )
	{
		const ::collada_to_protobuf::instance_physics_model &instancePhysicsModel = physicsScene.instance_physics_models_( i );
		
		// find the physics model definition that this physics model instance references
		::collada_to_protobuf::physics_model *defPhysicsModel = NULL;
		for( int k = 0; physics_model_library_.physics_models__size(); k++ )
		{
			::collada_to_protobuf::physics_model *tmpDefPhysicsModel = physics_model_library_.mutable_physics_models_( k );
			
			uint32_t defHash = tmpDefPhysicsModel->attr_().hash_id_();
			uint32_t insHash = instancePhysicsModel.url_().hash_id_();
			
			if( defHash == insHash )
			{
				defPhysicsModel = tmpDefPhysicsModel;
				break;	// found it
			}
			
		} // for each physics model
		
		// if couldn't find the physics model definition then skip
		if( defPhysicsModel == NULL )
			continue;
		
		// a physics model can have multiple rigid bodies so go through each one
		// NOTE: there is no corresponding equivalent in bullet for a physics model.  Its just a COLLADA thang...
		for ( int r = 0; r < instancePhysicsModel.rigid_body_instances__size(); r++ ) 
		{
			const ::collada_to_protobuf::instance_rigid_body &instanceRigidBody = instancePhysicsModel.rigid_body_instances_( r );
			btVector3 instanceScale( instanceRigidBody.scale_().x(), instanceRigidBody.scale_().y(), instanceRigidBody.scale_().z() );
			
			// find the rigid body definition that the instance references
			::collada_to_protobuf::rigid_body *defRigidBody = NULL;
			for ( int k = 0; k < defPhysicsModel->rigid_bodies__size(); k++ ) 
			{
				::collada_to_protobuf::rigid_body *tmpDefRigidBody = defPhysicsModel->mutable_rigid_bodies_( k );
				uint32_t defHash = tmpDefRigidBody->attr_().hash_id_();
				uint32_t insHash = instanceRigidBody.body_().hash_id_();
				
				if( defHash == insHash )
				{
					defRigidBody = tmpDefRigidBody;
					break;	// found it
				}
				
			} // for each rigid body definition
			
			// if couldn't find the rigid body definition then skip
			if( defRigidBody == NULL )
				continue;
				
			// we have the rigid body definition so get its shape(s) and physics material(s) and we got everything to build a bullet rigid body
			
			// get the either a instance physics material or a physics material definition
			::collada_to_protobuf::physics_material *defPhysicsMaterial = NULL;
			if( defRigidBody->has_instance_physics_material_() )
			{
				for ( int k = 0; k < physics_material_library_.physics_materials__size(); k++ ) 
				{
					::collada_to_protobuf::physics_material *tmpDefPhysicsMaterial = physics_material_library_.mutable_physics_materials_( k );
					uint32_t defHash = tmpDefPhysicsMaterial->attr_().hash_id_();
					uint32_t insHash = defRigidBody->instance_physics_material_().attr_().hash_id_();
					
					if( defHash == insHash )
					{
						defPhysicsMaterial = tmpDefPhysicsMaterial;
						break;
					}
					
				}	// for each physics material
			}
			else if( defRigidBody->has_physics_material_() )
			{
				defPhysicsMaterial = defRigidBody->mutable_physics_material_();
			}
			
			// now go through all the shapes and create the bullet equivalents...
			btCompoundShape* compoundShape = new btCompoundShape();
			
			for( int k = 0; k < defRigidBody->shapes__size(); k++ )
			{
				const ::collada_to_protobuf::shape &shape = defRigidBody->shapes_( k );
				// get the shape local transform if it exists
				btTransform localTransform;
				localTransform.setIdentity();
				
				if( shape.has_local_position_() )
					localTransform.setOrigin( 
							btVector3( shape.local_position_().x(), shape.local_position_().y(), shape.local_position_().z() ) );
				
				if( shape.has_local_rotation_() )
					localTransform.setRotation(
							btQuaternion( shape.local_rotation_().x(), shape.local_rotation_().y(), shape.local_rotation_().z(), shape.local_rotation_().w() ) );
					
				
				btCollisionShape* newShape = NULL;
				if( shape.has_plane_() )
				{
					newShape = new btStaticPlaneShape( 
							btVector3( shape.plane_().equation_().x(), shape.plane_().equation_().y(), shape.plane_().equation_().z() )
							, shape.plane_().equation_().w() );
				}
				else if( shape.has_box_() )
				{
					
					newShape = new btBoxShape(
							btVector3( shape.box_().half_extents_().x(), shape.box_().half_extents_().y(), shape.box_().half_extents_().z() ) ); 
				}
				else if( shape.has_sphere_() )
				{
					newShape = new btSphereShape( shape.sphere_().radius_() );
				}
				// todo: rest of shapes
				
				// add the shape to the compound shape
				if( newShape )
				{
					newShape->setLocalScaling( instanceScale );
					compoundShape->addChildShape( localTransform, newShape );					
				}

			}
			
			// no shapes so skip
			if( compoundShape->getNumChildShapes() == 0 )
				continue;
				
			// get the transform
			btTransform startTransform;
			startTransform.setIdentity();
			
			if( instanceRigidBody.has_position_() )
				startTransform.setOrigin( 
										 btVector3( instanceRigidBody.position_().x(), instanceRigidBody.position_().y(), instanceRigidBody.position_().z() ) );
			
			if( instanceRigidBody.has_rotation_() )
				startTransform.setRotation(
										   btQuaternion( instanceRigidBody.rotation_().x(), instanceRigidBody.rotation_().y(), instanceRigidBody.rotation_().z(), instanceRigidBody.rotation_().w() ) );
			
			
			// create the bullet rigid body
			btScalar mass = defRigidBody->dynamic_() == true ? defRigidBody->mass_() : 0.0f;
			btVector3 localInertia(0,0,0);
			if ( mass > 0.0f )
				compoundShape->calculateLocalInertia(mass,localInertia);
			
			btDefaultMotionState* myMotionState = new btDefaultMotionState( startTransform );
			btRigidBody::btRigidBodyConstructionInfo rbInfo( mass, myMotionState, compoundShape, localInertia );
			btRigidBody* body = new btRigidBody( rbInfo );
			body->setActivationState( ISLAND_SLEEPING );
			
			m_dynamicsWorld->addRigidBody( body );
			body->setActivationState( ISLAND_SLEEPING );
			
			
		} // for each rigid body instance
		
	}	// for each physics model instance
	
#if 0	
	///create a few basic rigid bodies
	btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
	//	btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);
	
	m_collisionShapes.push_back(groundShape);
	
	btTransform groundTransform;
	groundTransform.setIdentity();
	groundTransform.setOrigin(btVector3(0,-50,0));
	
	//We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
	{
		btScalar mass(0.);
		
		//rigidbody is dynamic if and only if mass is non zero, otherwise static
		bool isDynamic = (mass != 0.f);
		
		btVector3 localInertia(0,0,0);
		if (isDynamic)
			groundShape->calculateLocalInertia(mass,localInertia);
		
		//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
		btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
		btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia);
		btRigidBody* body = new btRigidBody(rbInfo);
		
		//add the body to the dynamics world
		m_dynamicsWorld->addRigidBody(body);
	}
	
	
	{
		//create a few dynamic rigidbodies
		// Re-using the same collision is better for memory usage and performance
		
		btCollisionShape* colShape = new btBoxShape(btVector3(SCALING*1,SCALING*1,SCALING*1));
		//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
		m_collisionShapes.push_back(colShape);
		
		/// Create Dynamic Objects
		btTransform startTransform;
		startTransform.setIdentity();
		
		btScalar	mass(1.f);
		
		//rigidbody is dynamic if and only if mass is non zero, otherwise static
		bool isDynamic = (mass != 0.f);
		
		btVector3 localInertia(0,0,0);
		if (isDynamic)
			colShape->calculateLocalInertia(mass,localInertia);
		
		float start_x = START_POS_X - ARRAY_SIZE_X/2;
		float start_y = START_POS_Y;
		float start_z = START_POS_Z - ARRAY_SIZE_Z/2;
		
		for (int k=0;k<ARRAY_SIZE_Y;k++)
		{
			for (int i=0;i<ARRAY_SIZE_X;i++)
			{
				for(int j = 0;j<ARRAY_SIZE_Z;j++)
				{
					startTransform.setOrigin(SCALING*btVector3(
															   2.0*i + start_x,
															   20+2.0*k + start_y,
															   2.0*j + start_z));
					
					
					//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
					btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
					btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia);
					btRigidBody* body = new btRigidBody(rbInfo);
					body->setActivationState(ISLAND_SLEEPING);
					
					m_dynamicsWorld->addRigidBody(body);
					body->setActivationState(ISLAND_SLEEPING);
				}
			}
		}
	}
#endif	
	
	clientResetScene();
}


void	ProtoBufTest::exitPhysics()
{
	
	//cleanup in the reverse order of creation/initialization
	
	//remove the rigidbodies from the dynamics world and delete them
	int i;
	for (i=m_dynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--)
	{
		btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
		btRigidBody* body = btRigidBody::upcast(obj);
		if (body && body->getMotionState())
		{
			delete body->getMotionState();
		}
		m_dynamicsWorld->removeCollisionObject( obj );
		delete obj;
	}
	
	//delete collision shapes
	for (int j=0;j<m_collisionShapes.size();j++)
	{
		btCollisionShape* shape = m_collisionShapes[j];
		delete shape;
	}
	
	delete m_dynamicsWorld;
	
	delete m_solver;
	
	delete m_broadphase;
	
	delete m_dispatcher;
	
	delete m_collisionConfiguration;
	
	
}




